﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;

namespace LearningBubbles
{
    class KinectControl
    {
        private KinectSensor kinectSensor;

        public KinectSensor KinectSensor
        {
            get { return kinectSensor; }
            set { kinectSensor = value; }
        }

        private string connectedStatus;

        public string ConnectedStatus
        {
            get { return connectedStatus; }
            set { connectedStatus = value; }
        }

        private Skeleton[] skeletons = new Skeleton[2];

        public Skeleton[] Skeletons
        {
            get { return skeletons; }
            set { skeletons = value; }
        }

        //billedstream evenhandler
        void kinectSensor_ColorFrameReady(object sender, ColorImageFrameReadyEventArgs e)
        {
            using (ColorImageFrame colorImageFrame = e.OpenColorImageFrame())
            {
                if (colorImageFrame != null)
                {
                    byte[] pixelsFromFrame = new byte[colorImageFrame.PixelDataLength];

                    colorImageFrame.CopyPixelDataTo(pixelsFromFrame);

                    Color[] color = new Color[colorImageFrame.Height * colorImageFrame.Width];
                    Textures.KinectRGBVideo = new Texture2D(Game1.Graphics.GraphicsDevice, colorImageFrame.Width, colorImageFrame.Height);

                    int index = 0;
                    for (int y = 0; y < colorImageFrame.Height; y++)
                    {
                        for (int x = 0; x < colorImageFrame.Width; x++, index += 4)
                        {
                            color[y * colorImageFrame.Width + x] = new Color(pixelsFromFrame[index + 2], pixelsFromFrame[index + 1], pixelsFromFrame[index + 0]);
                        }
                    }

                    //Textures.KinectRGBVideo.SetData(color);
                }
            }
        }

        //skeletonstream eventhandler
        void kinectSensor_SkeletonFrameReady(object Sender, SkeletonFrameReadyEventArgs e)
        {
            using (SkeletonFrame skeletonframe = e.OpenSkeletonFrame())
            {
                if (skeletonframe != null)
                {
                    Skeleton[] skeletonData = new Skeleton[skeletonframe.SkeletonArrayLength];
                    skeletonframe.CopySkeletonDataTo(skeletonData);
                    for (int skelNum = 0; skelNum < 2; skelNum++)
                    {
                        Skeleton playerSkeleton = (from s in skeletonData where s.TrackingState == SkeletonTrackingState.Tracked select s).FirstOrDefault();
                        if (playerSkeleton != null)
                        {
                            //    SkeletonPoint leftHand = playerSkeleton.Joints[JointType.HandLeft].Position;
                            //    //lHandPosition = new Vector2((((0.5f * leftHand.Position.X) + 0.5f) * (640)), (((-0.5f * leftHand.Position.Y) + 0.5f) * (480)));
                            //    ColorImagePoint cloc = kinectSensor.MapSkeletonPointToColor(leftHand, ColorImageFormat.RgbResolution640x480Fps30);
                            //    lHandPosition[skelNum] = new Vector2(cloc.X, cloc.Y);
                            //    SkeletonPoint rightHand = playerSkeleton.Joints[JointType.HandRight].Position;
                            //    ColorImagePoint cloc2 = kinectSensor.MapSkeletonPointToColor(rightHand, ColorImageFormat.RgbResolution640x480Fps30);
                            //    rHandPosition[skelNum] = new Vector2(cloc2.X, cloc2.Y);
                            //    //rHandPosition = new Vector2((((0.5f * rightHand.Position.X) + 0.5f) * (640)), (((-0.5f * rightHand.Position.Y) + 0.5f) * (480)));
                            skeletons[skelNum] = playerSkeleton;
                        }

                    }
                }
            }
        }
        public Vector3 lHandOffsetPos;
        public Vector2 lWristColorPoint1;
        public Vector2 lHandDepthPos;
        int initDistanceL = 0;
        int initDistanceR = 0;
        int newDistanceL;
        int newDistanceR;

        private MouseState ms;
        void kinectSensor_DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
        {
            using (DepthImageFrame imageFrame = e.OpenDepthImageFrame())
            {
                for (int skelNum = 0; skelNum < 1; skelNum++)
                {
                    if (skeletons[skelNum] != null && imageFrame != null)
                    {
                        DepthImagePoint lHandDepthPoint = imageFrame.MapFromSkeletonPoint(skeletons[skelNum].Joints[JointType.HandLeft].Position);
                        DepthImagePoint rHandDepthPoint = imageFrame.MapFromSkeletonPoint(skeletons[skelNum].Joints[JointType.HandRight].Position);
                        DepthImagePoint lWristDepthPoint = imageFrame.MapFromSkeletonPoint(skeletons[skelNum].Joints[JointType.WristLeft].Position);
                        DepthImagePoint rWristDepthPoint = imageFrame.MapFromSkeletonPoint(skeletons[skelNum].Joints[JointType.WristRight].Position);
                        lHandDepthPos = new Vector2(lHandDepthPoint.X, lHandDepthPoint.Y);

                        ColorImagePoint lHandColorPoint = imageFrame.MapToColorImagePoint(lHandDepthPoint.X, lHandDepthPoint.Y, ColorImageFormat.RgbResolution640x480Fps30);
                        ColorImagePoint rHandColorPoint = imageFrame.MapToColorImagePoint(rHandDepthPoint.X, rHandDepthPoint.Y, ColorImageFormat.RgbResolution640x480Fps30);
                        ColorImagePoint lWristColorPoint = imageFrame.MapToColorImagePoint(lWristDepthPoint.X, lWristDepthPoint.Y, ColorImageFormat.RgbResolution640x480Fps30);
                        ColorImagePoint rWristColorPoint = imageFrame.MapToColorImagePoint(rWristDepthPoint.X, rWristDepthPoint.Y, ColorImageFormat.RgbResolution640x480Fps30);

                        if(initDistanceL == 0)
                            initDistanceL = GetDistance(lHandDepthPoint, lWristDepthPoint);
                        if(initDistanceR == 0)
                            initDistanceR = GetDistance(rHandDepthPoint, rWristDepthPoint);

                        PlayerData.HandPosition[0, skelNum] = new Vector2(lHandColorPoint.X, lHandColorPoint.Y);
                        PlayerData.HandPosition[1, skelNum] = new Vector2(rHandColorPoint.X, rHandColorPoint.Y);

                        lWristColorPoint1 = new Vector2(lWristColorPoint.X, lWristColorPoint.Y);

                        PlayerData.LHandDepth[skelNum] = lHandDepthPoint.Depth;
                        PlayerData.RHandDepth[skelNum] = rHandDepthPoint.Depth;

                        PlayerData.WristPosition[0, skelNum] = new Vector2(lWristColorPoint.X, lWristColorPoint.Y);
                        PlayerData.WristPosition[1, skelNum] = new Vector2(rWristColorPoint.X, lWristColorPoint.Y);

                        //lHandOffsetPos = GetOffsetPos(lHandDepthPoint, lWristDepthPoint, imageFrame);
                        //Vector3 rHandOffsetPos = GetOffsetPos(rHandDepthPoint, rWristDepthPoint, imageFrame);

                        //PlayerData.HandGrabs[1, 0] = NewDetectFist(rHandDepthPoint, rWristDepthPoint, imageFrame);
                        ms = Mouse.GetState();

                        if (ms.LeftButton == ButtonState.Pressed)
                        {
                            PlayerData.HandGrabs[1, skelNum] = true;
                        }
                        else
                        {
                            PlayerData.HandGrabs[1, skelNum] = false;
                        }

                        //PlayerData.HandGrabs[1, 0] = SimpleDetectFist(rHandDepthPoint, rWristDepthPoint);
                        //PlayerData.HandGrabs[0, 0] = SimpleDetectFist(lHandDepthPoint, lWristDepthPoint);
                        //DetectFist(lHandDepthPoint, imageFrame);
                        //CheckGrab(lHandDepthPoint, lHandOffsetPos);
                        //PlayerData.HandGrabs[1, skelNum] = SimpleDetectFist(rHandDepthPoint,rWristDepthPoint);
                        //DetectFist(rHandDepthPoint, imageFrame);
                        //CheckGrab(rHandDepthPoint, rHandOffsetPos);

                        //lHandDepth = getValue(imageFrame, (int)lHandPosition[skelNum].X, (int)lHandPosition[skelNum].Y);
                        //rHandDepth = getValue(imageFrame, (int)rHandPosition[skelNum].X, (int)rHandPosition[skelNum].Y);
                    }
                }
            }
            //for (int skelNum = 0; skelNum < 2; skelNum++)
            //{
            //    if (imageFrame != null)
            //    {
            //        lHandDepth = getValue(imageFrame, (int)lHandPosition[skelNum].X, (int)lHandPosition[skelNum].Y);
            //        rHandDepth = getValue(imageFrame, (int)rHandPosition[skelNum].X, (int)rHandPosition[skelNum].Y);
            //    }
            //}
        }

        public Rectangle rect = new Rectangle(0,0,10,10);

        /// <summary>
        /// Unfinished new effort for detecting
        /// the hand making a fist.
        /// Firstly it finds the bounds of the hand.
        /// It checks how wide and how tall it is.
        /// Then matches the aspect ratio - if the width
        /// and height are about the same, return true.
        /// If not, return false.
        /// </summary>
        /// <param name="hand"></param>
        /// <param name="wrist"></param>
        /// <param name="imageFrame"></param>
        /// <returns></returns>
        bool NewDetectFist(DepthImagePoint hand, DepthImagePoint wrist, DepthImageFrame imageFrame)
        {
            float slope = GetSlope(new Vector2(wrist.X, wrist.Y), new Vector2(hand.X, hand.Y));
            float ortogonal = -1 / slope;
            short[] pixelData = new short[imageFrame.PixelDataLength];
            imageFrame.CopyPixelDataTo(pixelData);

            Vector3 handOffsetPoint = new Vector3(hand.X,hand.Y,hand.Depth);
            Vector3 fingersOffsetPoint = handOffsetPoint;

            rect.X = hand.X;
            rect.Y = hand.Y;

            bool handOnEdge = false;

            while (!handOnEdge)
            {
                if (wrist.X < hand.X)
                    handOffsetPoint.X += 1;
                else
                    handOffsetPoint.X -= 1;
                handOffsetPoint.Y -= 1 * ortogonal;
                handOffsetPoint.Z = GetDepthValue2(imageFrame,pixelData,(int)handOffsetPoint.X,(int)handOffsetPoint.Y);
                if ((handOffsetPoint.Z - hand.Depth) > 100)
                    handOnEdge = true;
            }

            DepthImagePoint hosp = new DepthImagePoint();
            hosp.X = (int)handOffsetPoint.X;
            hosp.Y = (int)handOffsetPoint.Y;

            int distance = GetDistance(hand, hosp);

            if (distance < 640) rect.Width = distance;

            bool handOnEdge1 = false;

            while (!handOnEdge1)
            {
                if (wrist.X < hand.X)
                    fingersOffsetPoint.X += 1;
                else
                    fingersOffsetPoint.X -= 1;
                fingersOffsetPoint.Y -= 1;
                fingersOffsetPoint.Z = GetDepthValue2(imageFrame, pixelData, (int)fingersOffsetPoint.X, (int)fingersOffsetPoint.Y);
                if ((fingersOffsetPoint.Z - hand.Depth) > 100)
                    handOnEdge1 = true;
            }

            DepthImagePoint hosp1 = new DepthImagePoint();
            hosp1.X = (int)fingersOffsetPoint.X;
            hosp1.Y = (int)fingersOffsetPoint.Y;

            int distance1 = GetDistance(hand, hosp1);

            rect.Height = distance1;

            int diff = Math.Abs(distance - distance1);

            if (diff >= 5 & distance < distance1) return false;
            else return true;
            //return false;
        }

        /// <summary>
        /// Gets the slope between coordinates
        /// </summary>
        /// <param name="v1"></param>
        /// <param name="v2"></param>
        /// <returns>slope</returns>
        float GetSlope(Vector2 v1, Vector2 v2)
        {
            float a = v2.Y - v1.Y;
            float b = v2.X - v1.X;
            if(b != 0)
                return a / b;
            else return 0;
        }

        /// <summary>
        /// Hand is making a fist, if the depth of
        /// the hand point is -35 or less than the
        /// depth of the wrist.
        /// </summary>
        /// <param name="hand"></param>
        /// <param name="wrist"></param>
        /// <returns>true if hand makes fist, false if not</returns>
        bool SimpleDetectFist(DepthImagePoint hand, DepthImagePoint wrist)
        {
            int difference = hand.Depth - wrist.Depth;
            if (difference < 0) difference -= 2 * difference;
            if (difference > 35) return true;
            else return false;
        }
        
        /// <summary>
        /// Get the distance between two pixels points.
        /// </summary>
        /// <param name="p1"></param>
        /// <param name="p2"></param>
        /// <returns></returns>
        public int GetDistance(DepthImagePoint p1, DepthImagePoint p2)
        {
            int a = p1.X - p2.X;
            if (a < 0) a -= 2 * a;
            int b = p1.Y - p2.Y;
            if (b < 0) b -= 2 * b;
            return (int)Math.Sqrt(Math.Pow(a, 2) + Math.Pow(b, 2));
        }

        /// <summary>
        /// returns the depth value at a certain pixel
        /// area of the depth image. This one both retains
        /// pixel data and finds the depth of a pixel. GetDepthValue2
        /// is better.
        /// </summary>
        /// <param name="imageFrame">The depth image frame</param>
        /// <param name="pixelData">pixel data from the frame</param>
        /// <param name="x">pixel coordinate</param>
        /// <param name="y">pixel coordinate</param>
        /// <returns>a depth value</returns>
        int GetDepthValue(DepthImageFrame imageFrame, int x, int y)
        {
            short[] pixelData = new short[imageFrame.PixelDataLength];
            imageFrame.CopyPixelDataTo(pixelData);
            return ((ushort)
              pixelData[x + y * imageFrame.Width]) >> 3;
        }

        /// <summary>
        /// returns the depth value at a certain pixel
        /// area of the depth image.
        /// </summary>
        /// <param name="imageFrame">The depth image frame</param>
        /// <param name="pixelData">pixel data from the frame</param>
        /// <param name="x">pixel coordinate</param>
        /// <param name="y">pixel coordinate</param>
        /// <returns>a depth value</returns>
        int GetDepthValue2(DepthImageFrame imageFrame, short[] pixelData, int x, int y)
        {
            if (y < 0) y = 0;
            else if (y > imageFrame.Height) y = imageFrame.Height;
            if (x < 0) x = 0;
            else if (x > imageFrame.Width) x = imageFrame.Width;
            return ((ushort)
              pixelData[x + y * imageFrame.Width]) >> 3;
        }


        /// <summary>
        /// This method makes crops a certain amount
        /// of pixels around the hand. Then it chekcs
        /// the amount of coverage - if the hand covers
        /// few of the pixels, the hand is making a fist
        /// thus it returns true, else not.
        /// </summary>
        /// <param name="handDepthPoint"></param>
        /// <param name="imageFrame"></param>
        /// <returns>true if covers</returns>
        public bool DetectFistByCoverageAmount(DepthImagePoint handDepthPoint, DepthImageFrame imageFrame)
        {
            int offsetParam = 12;
            int halfOffsetParam = offsetParam / 2;
            int[,] depths = new int[offsetParam, offsetParam];

            Vector2 point = new Vector2(handDepthPoint.X - offsetParam, handDepthPoint.Y - offsetParam);

            short[] pixelData = new short[imageFrame.PixelDataLength];
            imageFrame.CopyPixelDataTo(pixelData);

            int count = 0;
            int difference = 25;

            for (int i = 0; i < offsetParam; i++)
            {
                for (int j = 0; j < offsetParam; j++)
                {
                    //if (handDepthPoint.X <= offsetParam) handDepthPoint.X = offsetParam;
                    //else if (handDepthPoint.X >= imageFrame.Width - offsetParam) handDepthPoint.X = imageFrame.Width - offsetParam;
                    //if (handDepthPoint.Y <= offsetParam) handDepthPoint.Y = offsetParam;
                    //else if (handDepthPoint.Y >= imageFrame.Height - offsetParam) handDepthPoint.X = imageFrame.Height - offsetParam;

                    if (point.X < 0) point.X = 0;
                    else if (point.X > imageFrame.Width) point.X = imageFrame.Width;
                    if (point.Y < 0) point.Y = 0;
                    else if (point.Y > imageFrame.Width) point.Y = imageFrame.Height;

                    depths[i, j] = GetDepthValue2(imageFrame, pixelData, (int)point.X + i, (int)point.Y + j);

                    if (depths[i, j] - handDepthPoint.Depth > difference)
                    {
                        count++;
                    }
                }
            }

            if (count > depths.Length/2)
            {
                return true;
            }
            else return false;
        }

        /// <summary>
        /// Takes the current position of the hand and the wrist
        /// the distance between the two, adds that to the hand position
        /// assigning it to the offset position which is where
        /// it has to detect the fingers.
        /// </summary>
        /// <param name="handPosition">Depth position of the hand</param>
        /// <param name="wristPosition">Depth position of the wrist</param>
        /// <param name="imageFrame">Depth image frame</param>
        /// <returns>Offset position and depth of the position</returns>
        private Vector3 GetOffsetPos(DepthImagePoint handPosition, DepthImagePoint wristPosition, DepthImageFrame imageFrame)
        {
            Vector2 difference = new Vector2();




            if (handPosition.X < wristPosition.X) difference.X = wristPosition.X - handPosition.X;
            else difference.X = handPosition.X - wristPosition.X;

            if (handPosition.Y < wristPosition.Y) difference.Y = wristPosition.Y - handPosition.Y;
            else difference.Y = handPosition.Y - wristPosition.Y;

            Vector3 tempPos;
            if (handPosition.X < wristPosition.X && handPosition.Y < wristPosition.Y)
                tempPos = new Vector3((handPosition.X - (int)difference.X), (handPosition.Y - (int)difference.Y), 0);
            else if (handPosition.X < wristPosition.X && handPosition.Y > wristPosition.Y)
                tempPos = new Vector3((handPosition.X - (int)difference.X), (handPosition.Y + (int)difference.Y), 0);
            else if (handPosition.X > wristPosition.X && handPosition.Y < wristPosition.Y)
                tempPos = new Vector3((handPosition.X + (int)difference.X), (handPosition.Y - (int)difference.Y), 0);
            else tempPos = new Vector3((handPosition.X + (int)difference.X), (handPosition.Y + (int)difference.Y), 0);




            if (tempPos.X < 0) tempPos.X = 0;
            if (tempPos.Y < 0) tempPos.Y = 0;

            tempPos.Z = GetDepthValue(imageFrame, (int)tempPos.X, (int)tempPos.Y);

            return tempPos;
        }

        /// <summary>
        /// Matches the depth of the hand with the
        /// depth of the offset, returns true if
        /// the difference is above 100 milimeters
        /// which means the fingers are gone/hand is closed.
        /// </summary>
        /// <param name="handPosition">Depth position of the hand</param>
        /// <param name="offSetPos">Position of the offset</param>
        /// <returns>True if hand is grabbing, false if not</returns>
        private bool CheckGrabViaOffset(DepthImagePoint handPosition, Vector3 offSetPos)
        {
            int difference;
            if (handPosition.Depth < offSetPos.Z) difference = (int)offSetPos.Z - handPosition.Depth;
            else difference = handPosition.Depth - (int)offSetPos.Z;

            if (difference > 100)
                return true;
            else return false;
        }

        public void KinectSensors_StatusChanged(object sender, StatusChangedEventArgs e)
        {
            if (this.kinectSensor == e.Sensor)
            {
                if (e.Status == KinectStatus.Disconnected || e.Status == KinectStatus.NotPowered)
                {
                    this.kinectSensor = null;
                    this.DiscoverKinectSensor();
                }
            }
        }

        public void DiscoverKinectSensor()
        {
            foreach (KinectSensor sensor in KinectSensor.KinectSensors)
            {
                if (sensor.Status == KinectStatus.Connected)
                {
                    kinectSensor = sensor;
                    break;
                }
            }

            if (this.kinectSensor == null)
            {
                connectedStatus = "Found no Kinect Sensors connected to USB";
                return;
            }

            switch (kinectSensor.Status)
            {
                case KinectStatus.Connected:
                    {
                        connectedStatus = "Status: Connected";
                        break;
                    }
                case KinectStatus.Disconnected:
                    {
                        connectedStatus = "Status: Disconnected";
                        break;
                    }
                case KinectStatus.NotPowered:
                    {
                        connectedStatus = "Status: Not Powered";
                        break;
                    }
                default:
                    {
                        connectedStatus = "Status: Error";
                        break;
                    }
            }

            if (kinectSensor.Status == KinectStatus.Connected)
            {
                InitializeKinect();
            }
        }

        /// <summary>
        /// Initialize streams of the Kinect sensor
        /// </summary>
        /// <returns></returns>
        private bool InitializeKinect()
        {
            //get color stream
            kinectSensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
            kinectSensor.ColorFrameReady += new EventHandler<ColorImageFrameReadyEventArgs>(kinectSensor_ColorFrameReady);

            //get depth stream
            kinectSensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
            kinectSensor.DepthFrameReady += new EventHandler<DepthImageFrameReadyEventArgs>(kinectSensor_DepthFrameReady);

            //get skeleton stream
            kinectSensor.SkeletonStream.Enable(new TransformSmoothParameters()
            {
                Smoothing = 0.5f,
                Correction = 0.3f,
                Prediction = 0.3f,
                JitterRadius = 0.3f,
                MaxDeviationRadius = 0.04f
            });
            kinectSensor.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(kinectSensor_SkeletonFrameReady);

            try
            {
                kinectSensor.Start();
            }
            catch
            {
                connectedStatus = "Unable to start the Kinect Sensor";
                return false;
            }

            return true;
        }

    }
}
